#ifndef OOPSMP_UTILS_GEOMETRY_WORKSPACE_ARTICULATED_PART_3D_
#define OOPSMP_UTILS_GEOMETRY_WORKSPACE_ARTICULATED_PART_3D_

#include "OOPSMP/Utils/Geometry/Workspace/ArticulatedPart.h"
#include "OOPSMP/Utils/Geometry/Workspace/Part3D.H"

namespace OOPSMP {
	
	/**
     *@brief Macro for class, class pointer, and const class pointer declarations
     */
	ForwardClassDeclaration(ArticulatedPart3D);
	
	/**
     *@brief articulated part 3d
     */
	class ArticulatedPart3D : public ArticulatedPart {
	
	public:
		
		/**
		 *@brief Calculates all triangles associated with articulated part
		 */
		virtual void calcTotalTriangles() {

			for ( long i = 0 ; i < ct.getLinkCount() ; i++ )
				n_totalNrTriangles += dynamic_cast<Part3D_const_t>(ct.getNodeRef(i).getLink().getPart())->getNrTriangles();

			long nrTri = 0, nrV = 0;

			v_allTriangles.reserve( n_totalNrTriangles * 3 );

			for ( long i = 0 ; i < ct.getLinkCount() ; i++ ) {

				int numLinkTri = dynamic_cast<Part3D_const_t>(ct.getNode(i).getLink().getPart())->getNrTriangles();
				const int * temp = dynamic_cast<Part3D_const_t>(ct.getNode(i).getLink().getPart())->getTriangles();

				for ( int j = 0 ; j < numLinkTri ; j++ ) {
					v_allTriangles.push_back( temp[j * 3 + 0] + nrV );
					v_allTriangles.push_back( temp[j * 3 + 1] + nrV );
					v_allTriangles.push_back( temp[j * 3 + 2] + nrV );
				}

				nrV += dynamic_cast<Part3D_const_t>(ct.getNode(i).getLink().getPart())->getNrVertices();
								
			}

		}
		
		/**
		 *@brief Calculates all vertices associated with articulated part
		 */
		virtual void calcTotalVertices() {

			for ( long i = 0 ; i < ct.getLinkCount() ; i++ )
				n_totalNrVertices += dynamic_cast<Part3D_const_t>(ct.getNodeRef(i).getLink().getPart())->getNrVertices();

		}
		
		/**
		 *@brief Get (x, y, z) coordinates of the i-th vertex
		 *@param i vertex index
         */
		virtual const double * getVertex(const int i) const	{

			if ( i > getNrVertices() )
				return NULL;

			int linkID = 0, index = i;

			while ( index > dynamic_cast<Part3D_const_t>(ct.getNode(linkID).getLink().getPart())->getNrVertices() ) {
				index -= dynamic_cast<Part3D_const_t>(ct.getNode(linkID).getLink().getPart())->getNrVertices();
				linkID += 1;
			}

			return dynamic_cast<Part3D_const_t>(ct.getNode(linkID).getLink().getPart())->getVertex(index);
		}
		
		/**
		 *@brief Get a copy of the (x, y, z) coordinates of the i-th vertex
	     *@param i vertex index
	     *@param v vertex
         */
		virtual void getVertex(const int i, OOPSMPvector3_t v) const
		{
			assert(v != NULL);

			const double *vorig = getVertex(i);

			v[0] = vorig[0];
			v[1] = vorig[1];
			v[2] = vorig[2];
		}
		
		/**
		 *@brief Get the (x, y, z) coordinates of the i-th vertex normal
		 *@param i vertex index
         */
		virtual OOPSMPconst_vector3_t getNormal(const int i)
		{
			assert(i >= 0 && i < getNrVertices());	    
			
			int linkID = 0, index = i;

			while ( index > dynamic_cast<Part3D_const_t>(ct.getNode(linkID).getLink().getPart())->getNrVertices() ) {
				index -= dynamic_cast<Part3D_const_t>(ct.getNode(linkID).getLink().getPart())->getNrVertices();
				linkID += 1;
			}

			return dynamic_cast<Part3D_t>(ct.getNode(linkID).getLink().getPart())->getNormal( index );
		}
		
		/**
		 *@brief Get a copy of the (x, y, z) coordinates of the i-th vertex normal
		 *@param i vertex index
		 *@param nv vertex normal
         */
		virtual void getNormal(const int i, OOPSMPvector3_t nv)
		{
			assert(nv != NULL);	    
			OOPSMPconst_vector3_t norig = getNormal(i);	    
			nv[0] = norig[0];
			nv[1] = norig[1];
			nv[2] = norig[2]; 
		}
		
		/**
		 *@brief Returns point belonging to physical link
		 *@param i link index
		 *@param out empty array to contain point
         */
		virtual void  getSomePointForLink( const int i, double * out ) {
			ct.getNode(i).getLink().getPart()->getSomePoint( out );
		}

		/*bool areInCollision(double *bbox_a, double *bbox_b) const
		{
			for (int i = 0; i < 3; i++)
				if ((bbox_a[i] > bbox_b[i+3]) || (bbox_a[i+3] < bbox_b[i])) // boxes do not overlap at least on one axis
					return false;
			return true;
		}*/
		
		/**
		 *@brief Returns true iff two bboxes are in collision
		 *@param bbox_a bbox of chain tree node
		 *@param bbox_b bbox of chain tree node
         */
		bool areBBoxesInCollision(double *bbox_a, double *bbox_b) const
		{
			for (int i = 0; i < 3; i++) {
				if ((bbox_a[i] > bbox_b[i+3]) || (bbox_a[i+3] < bbox_b[i])) { 
					// boxes do not overlap at least on one axis
					return false;
				}
			}

			return true;
		}
		
		/**
		 *@brief The radius of the part is defined as the maximum distance
		 *       from any original part vertex to the origin
		 */
		virtual double computeRadiusSquare(void) const {
			double        dmax = 0.0;
			double        d    = 0.0;

			for(int i = 0; i < n_totalNrVertices; i++) {

				OOPSMPdeclare_vector3(v);
				OOPSMPcopy_vector3( v, getVertex(i) );
				d = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
				if( d > dmax ) dmax = d;

			}

			return dmax;
		}
		
		/**
		 *@brief Use standard material for moving part
		 */
		void useStandardMaterialForMovingPart(void)
		{
			if(getMaterial() == NULL)
				setMaterial(new Material3D());
			getMaterial()->standardForMovingPart();
		}
		
		/**
		 *@brief Use standard material for static part
         */
		void useStandardMaterialForStaticPart(void)
		{
			if(getMaterial() == NULL)
				setMaterial(new Material3D());
			getMaterial()->standardForStaticPart();
		}
		
		/**
		 *@brief Returns dimension of articulated part
         */
		int getPartDim() const { return 3; }

		/**
		 *@brief Returns point at center of joint associated with physical link
		 *@param i link index
		 *@param jointCenterPoint empty array to contain point
         */
		void getJointCenterPointForLink( const int i, double * jointCenterPoint ) const {
		
			// we need link's (0,0,0), its dimensions and its jointLength (joint precedes the link)
			double * linkRefPoint = OOPSMPalloc_vector3();
			ct.getNode(i).getLink().getLinkRefPoint(linkRefPoint);
			double * linkDimensions = OOPSMPalloc_vector3();
			ct.getNode(i).getLink().getLinkDimensions(linkDimensions);
			double jointLength = ct.getNode(i).getLink().getJoint()->getJointLength();

			// now, calculate joint's elbow according to the following formula:
			// ( x0 - jointLengh, y0 + yDIM / 2, z0 + zDIM / 2 )
			jointCenterPoint[0] = linkRefPoint[0] - jointLength / 2;
			jointCenterPoint[1] = linkRefPoint[1] + linkDimensions[1] / 2;
			jointCenterPoint[2] = linkRefPoint[2] + linkDimensions[2] / 2;

			OOPSMPfree_vector3( linkDimensions );
			OOPSMPfree_vector3( linkRefPoint );
		}
		
		/**
		 *@brief Returns length of joint associated with physical link
		 *@param i link index
		 */
		double getJointLengthForLink( const int i ) {
			return ct.getNode(i).getLink().getJoint()->getJointLength();
		}
		
		/**
		 *@brief Returns length, width, depth of physical link
		 *@param i link index
		 */
		void getDimensionsForLink( const int i, double * lnkDims ) {
			return ct.getNode(i).getLink().getLinkDimensions( lnkDims );
		}

	};
	
	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */
	DeclareInstanceFactory(ArticulatedPart3D, ArticulatedPartFactory);

}

#endif
